//=============================================================================
//
// StopGoBtMotorPart.cpp : "Stop and go via Bluetooth (motor part)"
//
// This example starts and stops motor, connected to outputs M1, by means
// of the button, connected to the input I8 on other ROBO TX Controller.
// Pulses from the motor are calculated by the counter C1. The motor is
// stopped after the counter reaches the value of 1000.
//
//-----------------------------------------------------------------------------
// Disclaimer - Exclusion of Liability
//
// This software is distributed in the hope that it will be useful,but WITHOUT 
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
// FITNESS FOR A PARTICULAR PURPOSE. It can be used an modified by anyone
// free of any license obligations or authoring rights.
//=============================================================================

#include "StdAfx.h"

#define MOTOR_NUMBER    1
#define MOTOR_IDX       (MOTOR_NUMBER - 1)

#define BT_CHANNEL      1

//  Bluetooth address of other ROBO TX Controller
static BYTE * bt_address = bt_address_table[0];

#define MOTOR_ON        TRUE
#define MOTOR_OFF       FALSE

#define STRMAXLEN       80

static HANDLE           fthdl;
static DWORD            errCode;
static char             ComPortName[STRMAXLEN];
static char             LibVersion[STRMAXLEN];

static enum bt_commands_e command;
static volatile CHAR8 command_status;
static volatile CHAR8 receive_command_status;


//=============================================================================

/*-----------------------------------------------------------------------------
 * Function Name       : BtCallback
 *
 * This callback function is called to inform the program about result (status)
 * of execution of any Bluetooth command except BtReadMsgOn command.
 *-----------------------------------------------------------------------------*/
static void __stdcall BtCallback(BT_CB *p_data) {

    command_status = (CHAR8)p_data->status;
}

/*-----------------------------------------------------------------------------
 * Function Name       : BtReceiveCallback
 *
 * This callback function is called to inform the program about result (status)
 * of execution of BtReadMsgOn command. It is also called when a message
 * arrives via Bluetooth.
 *-----------------------------------------------------------------------------*/
static void __stdcall BtReceiveCallback(BT_RECV_CB * p_data) {

    if (p_data->status == BT_MSG_INDICATION) {

        UCHAR8 motor;
        INT16 duty;
        UCHAR8 msg[3];
        INT16 count;
        INT16 cntmode;

        // Format of a received message should be:
        // byte 0  : motor number(1...N_MOTOR)
        // byte 1-2: motor duty(DUTY_MIN...DUTY_MAX)
        motor = p_data->msg[0];
        if (motor >= 1 && motor <= N_MOTOR) {
            memcpy(&duty, &p_data->msg[1], sizeof(duty));
            if (duty >= DUTY_MIN && duty <= DUTY_MAX) {
                SetOutMotorValues(fthdl, TA_LOCAL, motor - 1, duty, 0);
            }

            //  prepare reply BT message
            msg[0] = motor;                         // counter number
            GetInCounterValue(fthdl, TA_LOCAL, motor - 1, &count, &cntmode);
            memcpy(&msg[1], &count, sizeof(count)); // counter value

            //  send BT message
            command = CMD_SEND;
            command_status = -1;
            SendBtMessage(fthdl, BT_CHANNEL, sizeof(msg), (LPSTR)msg, BtCallback);
        }
    }
    else {
        receive_command_status = (CHAR8)p_data->status;
    }
}

/*-----------------------------------------------------------------------------
 *  StopGoBtMotorPart  
 *---------------------------------------------------------------------------*/
static void StopGoBtMotorPart(void) {

    INT16 count;
    INT16 cntmode;

    //  configure motor output to be used as a motor output
    SetFtMotorConfig(fthdl, TA_LOCAL, MOTOR_IDX, MOTOR_ON);

    //  reset counter
    StartCounterReset(fthdl, TA_LOCAL, MOTOR_IDX);

    //  waiting for counter reset
    do {
        GetInCounterValue(fthdl, TA_LOCAL, MOTOR_IDX, &count, &cntmode);
        Sleep(1);
    } while (count > 0);

    //  start listen to the controller with bt_address via Bluetooth channel BT_CHANNEL
    command = CMD_START_LISTEN;
    command_status = -1;
    BtListenConOn(fthdl, BT_CHANNEL, bt_address, BtCallback);

    while (command_status < 0) Sleep(1);
    BtDisplayCommandStatus(bt_address, BT_CHANNEL, command, command_status);
    if (command_status != BT_SUCCESS)
        return;

    //  wait for incoming connection
    do {
        command = CMD_NO_CMD;
        command_status = -1;
        while (command_status < 0) Sleep(1);
        BtDisplayCommandStatus(bt_address, BT_CHANNEL, command, command_status);
    } while (command_status != BT_CON_INDICATION);

    //  start receive from Bluetooth channel BT_CHANNEL
    command = CMD_START_RECEIVE;
    receive_command_status = -1;
    BtReadMsgOn(fthdl, BT_CHANNEL, BtReceiveCallback);

    while (receive_command_status < 0) Sleep(1);
    BtDisplayCommandStatus(bt_address, BT_CHANNEL, command, receive_command_status);
    if (receive_command_status != BT_SUCCESS)
        return;

    command = CMD_NO_CMD;
    command_status = -1;

    while (1) {
        if (command_status >= 0) {
            if (command_status != BT_SUCCESS) {

                //  usually if we have come here, then this means the other controller
                //  has disconnected from us
                BtDisplayCommandStatus(bt_address, BT_CHANNEL, command, command_status);

                //  stop the motor
                SetOutMotorValues(fthdl, TA_LOCAL, MOTOR_IDX, DUTY_MIN, 0);

                //  reset counter to be prepared for the next time when
                //  other controller connects to us again
                StartCounterReset(fthdl, TA_LOCAL, MOTOR_IDX);

                //  waiting for counter reset
                do {
                    GetInCounterValue(fthdl, TA_LOCAL, MOTOR_IDX, &count, &cntmode);
                    Sleep(1);
                } while (count > 0);

                command_status = -1;

                /*
                //  this block of code should be used if we want that the program stops when
                //  other ROBO TX Controller disconnects from us
                return;
                */
            }
            command = CMD_NO_CMD;
        }
        Sleep(10);
    }
}

/*-----------------------------------------------------------------------------
 *  CheckParameter  
 *---------------------------------------------------------------------------*/
static int CheckCOMPar(int argc, char *argv[]) {

    char    *pStr;
    int     comNo;

    if (argc >= 2) {
        if (strlen(argv[1]) > 3) {
            if ((pStr=strstr(argv[1],"COM")) != NULL) {
                sscanf(pStr+3, "%d", &comNo);
                if (comNo >= 1 && comNo <= 255)
                    return 0;
                else {
                    cout << "StopGoBtMotorPart.exe: invalid COM number..." << endl << endl;
                    return 1;
                }
            }
        }
    }

    cout << "StopGoBtMotorPart.exe: no input given..." << endl << endl;
    return 1;
}

/*-----------------------------------------------------------------------------
 *  main
 *  
 *---------------------------------------------------------------------------*/
int main(int argc, char *argv[]) {

    cout << "\nExample StopGoBtMotorPart.exe ..." << endl;

    //  check input paramter
    if (CheckCOMPar(argc,argv)) {
        cout << "Usage: StopGoBtMotorPart.exe COMxx\t(e.g. COM2 or COM32)" << endl;
        return 1;
    }

    //  get library version
    ftxGetLibVersionStr(LibVersion, STRMAXLEN);
    cout << "\nftMscLib " << LibVersion << endl;

    //  library initialization
    errCode = ftxInitLib();

    strcpy(ComPortName, argv[1]);
    cout << "\nOpen ComPort '" << ComPortName << "' ..." << endl;

    //  open COM port
    fthdl = ftxOpenComDevice(ComPortName, 38400, &errCode);

    if (errCode == FTLIB_ERR_SUCCESS) {

        cout << "Connected to ROBO TX Controller ..." << endl;

        //  starting Transfer Area
        errCode = ftxStartTransferArea(fthdl);

        if (errCode == FTLIB_ERR_SUCCESS) {

            cout << "Transfer Area was started and runs..." << endl;

            StopGoBtMotorPart();

            //  stop Transfer Area
            ftxStopTransferArea(fthdl);
        }

        else {
            //  error case
            cout << "Error: Transfer Area was not started !" << endl;
        }

        //  closing port
        cout << "Closing ComPort '" << ComPortName << "' ..." << endl;
        errCode = ftxCloseDevice(fthdl);
    }

    else {
        //  error case
        cout << "Error: No interface available (Port '" << ComPortName << "')" << endl;
    }

    //  close library
    ftxCloseLib();

    return 0;
}
